/*
   StandInstance.cpp - Library for controlling a satellite tracking
   Stand device which includes two stepper motor drivers and connected
   stepper motors. This library provides functions that accept as input a pair
   of integer azimuth and elevation angle values.
   
   Pin connections are defined in PinConstants.h. The stepper motor that 
   controls the position in azimuth should be connected as motor driver 1 
   MD1). The stepper motor that controls the position in elevation should be 
   connected as motor driver 2 (MD2).
   
   Stepper motor rotation
   Stepper motors should be connected such that clockwise rotation (from the 
   perspective of the motor) will cause the position values to increase--
   that is, increasing elevation should raise the Stand toward zenith (90), 
   and increasing azimuth should turn the Stand clockwise (such as E to SE).
   
   Northern Hemisphere Mode
   When the Stand is operating in northern hemisphere mode, the Stand will 
   be allowed to move in the full range of -90 to 450 deg in azimuth. This code
   will automatically adjust the angles to choose the shortest path to an 
   azimuth angle enabling the Stand to reach any degree value greater than
   270 and less than 90 without spinning the whole way around through 180 
   degrees.
   
   Created by the STA Capstone team at UMBC on 24 January 2013. Final revisions
   made on 14 May 2013.

   Edited (very slightly) by the LSI team at UMBC on 7 March 2014.
*/

#include "Arduino.h"
#include "StandInstance.h"
#include "StepperDriver.h"
#include "Decoder.h"
#include "PinConstants.h"

#define AZ_MAXPOS 450      // highest position allowed in azimuth
#define AZ_MINPOS -90      // lowest position allowed in azimuth
#define AZ_CHANGEOVER 270  // position to change to northern hemisphere mode
#define AZ_MAXNHMODE 90    // maximum position that will be shifted in northern hemisphere mode
#define AZ_GEARRATIO 2.7273   // gear ratio for azimuth drivetrain
#define AZ_STEPSPERREV 200    // steps per revolution for azimuth stepper motor
#define AZ_DEGPERSTEP 1.8  // degrees per step for azimuth stepper motor
#define AZ_MICSTEPPERDEG 17.7778 //MICROsteps per degree
#define AZ_STARTINGSPS 30  // starting speed in SPS for azimuth rotation
#define AZ_MINSPS 10       // minimum speed in SPS for azimuth rotation
#define AZ_MAXSPS 1000     // maximum speed in SPS for azimuth rotation
#define AZ_SPSPS 1         // azimuth acceleration
#define AZ_DEFAULTSTEPSIZE 5  // default step size
#define EL_MICSTEPPERDEG 17.7778 //MICROsteps per degree
#define EL_MAXPOS 90       // highest position allowed in elevation
#define EL_MINPOS 0        // lowest position allowed in elevation
#define EL_GEARRATIO 48    // gear ratio for elevation drivetrain
#define EL_STEPSPERREV 150 // steps per revolution for elevation stepper motor
#define EL_DEGPERSTEP 1.8  // degrees per step for elevation stepper motor
#define EL_STARTINGSPS 100 // starting speed in SPS for elevation
#define EL_MINSPS 50       // minimum speed in SPS for elevation
#define EL_MAXSPS 2000     // maximum speed in SPS for azimuth rotation
#define EL_SPSPS 1         // elevation acceleration
#define EL_DEFAULTSTEPSIZE 4  // default step size for elevation


#define MAXAZBEFOREEL AZ_MAXPOS // maximum degrees in azimuth to move before moving in elevation
#define MAXELBEFOREAZ EL_MAXPOS // maximum degrees in elevation to move before moving in azimuth

#define ELRESETPOS 0  // elevation reset position
#define AZRESETPOS 0  // azimuth reset position

#define MICROSINSECONDS 1000000
#define MICROSINMILLIS 1000
#define MILLISINSECONDS 1000

#define STANDAZLOWBOUND 0
#define STANDAZHIGHBOUND 360
#define STANDALTLOWBOUND 0
#define STANDALTHIGHBOUND 90


static const int stepModeMultiplierToMicro[8] = {32,16,8,4,2,1,1,1};
static bool northHemisMode = false;

/******************************************************************************
   Constructors
******************************************************************************/
 /*
   SatTrackingStand constructor
   
   Required input parameters:
   - Initial position in azimuth (in degrees).
   - Initial position in elevation (in degrees).
 */
StandInstance::StandInstance(int azInitPos, int elInitPos)
{
   this->azPos = azInitPos;
   this->elPos = elInitPos;
   this->azDriver = new StepperDriver(MD1_STEP,MD1_DIR,MD1_SLP,MD1_RST,MD1_M0,MD1_M1,MD1_M2,MD1_EN);
   this->elDriver = new StepperDriver(MD2_STEP,MD2_DIR,MD2_SLP,MD2_RST,MD2_M0,MD2_M1,MD2_M2,MD2_EN);
   this->decoder = new Decoder();
   this->azDriver->disableDriver();
   this->elDriver->disableDriver();

   this->routeAzLowBound = STANDAZLOWBOUND;
   this->routeAzHighBound = STANDAZHIGHBOUND;
   this->routeAltLowBound = STANDALTLOWBOUND;
   this->routeAltHighBound = STANDALTHIGHBOUND;
}
 
int runRoute(int azLowBound, int azHighBound, int altLowBound, int altHighBound)
{
   // Setup Scanning Route
   assignRouteBounds(azLowBound, azHighBound, altLowBound, altHighBound);

   /*
   orient stand to lowAz, lowEl
   */
   moveTo(this->routeAzLowBound, this->routeAltLowBound);

   // Loop: Range, Relay and Route
   /* _   _   _
     | | | | | |
     | | | | | |
     | |_| |_| |
   



   
   
   */


   // Reset to (0,0)
   reset();

   return 0; // Notification that scan has successfully completed
}


void assignRouteBounds(int azLowBound, int azHighBound, int altLowBound, int altHighBound)
{
   //if any inputted scan bound exceeds stand bounds, set scan bound to stand bound 
   if((azLowBound >= STANDAZLOWBOUND) && (azLowBound <= azHighBound) && (azHighBound <= STANDAZHIGHBOUND)){
      this->routeAzLowBound = azLowBound;
   }
   else{
      this->routeAzLowBound = STANDAZLOWBOUND;
   }
   // Check AzHighBound
   if((azLowBound >= STANDAZLOWBOUND) && (azLowBound <= azHighBound) && (azHighBound <= STANDAZHIGHBOUND)){
      this->routeAzHighBound = azHighBound;
   }
   else{
      this->routeAzHighBound = STANDAZHIGHBOUND;
   }

   // Check AltLowBound
   if((altLowBound >= STANDALTLOWBOUND) && (altLowBound <= altHighBound) && (altHighBound <= STANDALTHIGHBOUND)){
      this->routeAltLowBound = altLowBound;
   }
   else{
      this->routeAltLowBound = STANDALTLOWBOUND;
   }

   // Check AltHighBound
   if((altLowBound >= STANDALTLOWBOUND) && (altLowBound <= altHighBound) && (altHighBound <= STANDALTHIGHBOUND)){
      this->routeAltHighBound = altHighBound;
   }
   else{
      this->routeAltHighBound = STANDALTHIGHBOUND;
   }
}



/******************************************************************************
   Movement commands
******************************************************************************/
void StandInstance::updatePosition(int newAz, int newEl) {
   this->azPos = newAz;
   this->elPos = newEl;
}


/*
   moveTo
   Moves the Stand to a new position (in degrees). If new position is
   invalid, an error statement is printed on Serial and move function is
   aborted.
   
   Required input parameters:
   - New azimuth position in degrees.
   - New elevation position in degrees.
*/
void StandInstance::moveTo(int newAz, int newEl)
{
   if(!isValidAzPos(newAz))
   {
      Serial.print(newAz);
      Serial.println(" is invalid azimuth position.");
      return;
   }
   
   if(!isValidElPos(newEl))
   {
      Serial.print(newEl);
      Serial.println(" is invalid elevation position.");
      return;
   }
   
   int azDegLeft = abs(newAz - this->azPos);
   int elDegLeft = abs(newEl - this->elPos);
   long nextAzMove = 0, nextElMove = 0;
   long azStepsMoved = 0, elStepsMoved = 0;
   
   // set azimuth direction
   if (newAz >= this->azPos) {
      this->azDriver->setDirection(StepperDriver::CLOCKWISE);
   } else {
      this->azDriver->setDirection(StepperDriver::COUNTERCLOCKWISE);
   }
   // set elevation direction
   if (newEl >= this->elPos) {
      this->elDriver->setDirection(StepperDriver::CLOCKWISE);
   } else {
      this->elDriver->setDirection(StepperDriver::COUNTERCLOCKWISE);
   }
   
   while (azDegLeft > 0 || elDegLeft > 0) {
      if (elDegLeft > MAXELBEFOREAZ && azDegLeft > 0) {
         nextElMove = MAXELBEFOREAZ;
      } else {
         nextElMove = elDegLeft;
      }
      
      if (nextElMove > 0) {
         Serial.print("Stepping ");
         Serial.print(nextElMove);
         Serial.print(" degrees in Elevation, ");
         Serial.print(this->elDegToStep(nextElMove));
         Serial.println(" micro-steps");
         elStepsMoved = this->elDriver->smartStep(this->elDegToStep(nextElMove),EL_STARTINGSPS,EL_SPSPS);
         updatePosition(this->azPos, this->elPos + nextElMove*this->elDriver->getDirection());
         elDegLeft -= nextElMove;
         nextElMove = 0;
      }
      
      
      if (azDegLeft > MAXAZBEFOREEL && elDegLeft > 0) {
         nextAzMove = MAXAZBEFOREEL;
      } else {
         nextAzMove = azDegLeft;
      }
      
      if (nextAzMove > 0) {
         Serial.print("Stepping ");
         Serial.print(nextAzMove);
         Serial.print(" degrees in Azimuth, ");
         Serial.print(this->azDegToStep(nextAzMove));
         Serial.println(" micro-steps");
         this->azDriver->smartStep(this->azDegToStep(nextAzMove),AZ_STARTINGSPS,AZ_SPSPS);
         updatePosition(this->azPos + nextAzMove*this->azDriver->getDirection(), this->elPos);
         azDegLeft -= nextAzMove;
         nextAzMove = 0;
      }

      Serial.print("elDegLeft=");
      Serial.print(elDegLeft);
      Serial.print("; azDegLeft=");
      Serial.println(azDegLeft);
   }
}




/*
   moveToSimultaneous
   Moves to the new azimuth and elevation values at a constant speed and in
   both directions simultaneously.
 */
/*
int StandInstance::moveToSimultaneous(int newAz, int newEl)
{
   if(!isValidAzPos(newAz))
   {
      Serial.print(newAz);
      Serial.println(" an is invalid azimuth position.");
      return 0;
   }
   
   if(!isValidElPos(newEl))
   {
      Serial.print(newEl);
      Serial.println(" is an invalid elevation position.");
      return 0;
   }
   
   if (newAz > AZ_CHANGEOVER) {
      if (!northHemisMode && this->azPos < AZ_MAXNHMODE) {
         newAz -= 360;
         Serial.println("Northern Hemisphere Negative Mode is on");
      } else {
         northHemisMode = true;
         Serial.println("Northern Hemisphere Positive Mode is on");
      }
   } else if (northHemisMode && newAz < AZ_MAXNHMODE) {
      newAz += 360;
      Serial.print("NH Positive Mode: azimuth adjusted to ");
      Serial.println(newAz);
   } else {
      northHemisMode = false;
      Serial.println("Northern Hemisphere Mode is off");
   }
   
   
   int azDegLeft = abs(newAz - this->azPos);
   int elDegLeft = abs(newEl - this->elPos);
   int azStepMode = AZ_DEFAULTSTEPSIZE; this->azDriver->setStepMode(azStepMode);
   int elStepMode = EL_DEFAULTSTEPSIZE; this->elDriver->setStepMode(elStepMode);
   long azStepsLeft = this->azDegToStep(azDegLeft) / stepModeMultiplierToMicro[azStepMode];
   long elStepsLeft = this->elDegToStep(elDegLeft) / stepModeMultiplierToMicro[elStepMode];
   long azStepDelay = 0;
   long elStepDelay = 0;
   long azSPS = AZ_STARTINGSPS, elSPS = EL_STARTINGSPS;
   
   // Set direction and print
   Serial.print("az change: ");
   // set azimuth direction
   if (newAz >= this->azPos) {
      this->azDriver->setDirection(StepperDriver::CLOCKWISE);
      Serial.print("+");
   } else {
      this->azDriver->setDirection(StepperDriver::COUNTERCLOCKWISE);
      Serial.print("-");
   }
   Serial.print(azDegLeft);
   Serial.print("; el change: ");
   // set elevation direction
   if (newEl >= this->elPos) {
      this->elDriver->setDirection(StepperDriver::CLOCKWISE);
      Serial.print("+");
   } else {
      this->elDriver->setDirection(StepperDriver::COUNTERCLOCKWISE);
      Serial.print("-");
   }
   Serial.print(elDegLeft);
   Serial.print("; azSteps: ");
   Serial.print(azStepsLeft);
   Serial.print("; elSteps: ");
   Serial.print(elStepsLeft);
   Serial.print("; azSPS: ");
   Serial.print(azSPS);
   Serial.print("; elSPS: ");
   Serial.println(elSPS);

   delay(100);
   this->azDriver->enableDriver();
   this->elDriver->enableDriver();
   delay(100);
   
   azStepDelay = floor(MICROSINSECONDS / azSPS);
   elStepDelay = floor(MICROSINSECONDS / elSPS);
 
   while(elStepsLeft > 0 || azStepsLeft > 0) {
      
      if (azStepsLeft == 0) {
         this->azDriver->disableDriver();
      }
      // step once in azimuth if az should step during this millisecond
      // move only if the appropriate delay has passed
      else if (azStepsLeft > 0 && micros() - this->azDriver->getTimeLastStep() >= azStepDelay) {
         // decrement the steps left
         azStepsLeft--;
         // step the motor, sets this->timeLastStep
         this->azDriver->stepMotor();
      }
      
      if (elStepsLeft == 0) {
         this->elDriver->disableDriver();
      }
      // step once in elevation if el should step during this millisecond
      // move only if the appropriate delay has passed
      else if (elStepsLeft > 0 && micros() - this->elDriver->getTimeLastStep() >= elStepDelay) {
         // decrement the steps left
         elStepsLeft--;
         // step the motor, sets this->timeLastStep
         this->elDriver->stepMotor();
      }
   }
   updatePosition(newAz, newEl);
   delay(100);
   this->azDriver->disableDriver();
   this->elDriver->disableDriver();
   
   return 1;
}
*/



/*
   moveToAccel
   Moves to the new azimuth and elevation values with acceleration 
   simultaneously in both directions.
 */
int StandInstance::moveToAccel(int newAz, int newEl, bool nhmode)
{
   if(!isValidAzPos(newAz))
   {
      Serial.print(newAz);
      Serial.println(" an is invalid azimuth position.");
      return 0;
   }
   
   if(!isValidElPos(newEl))
   {
      Serial.print(newEl);
      Serial.println(" is an invalid elevation position.");
      return 0;
   }
   
   if (nhmode && newAz > AZ_CHANGEOVER) {
      if (!northHemisMode && this->azPos < AZ_MAXNHMODE) {
         newAz -= 360;
         Serial.println("Northern Hemisphere Negative Mode is on");
      } else {
         northHemisMode = true;
         Serial.println("Northern Hemisphere Positive Mode is on");
      }
   } else if (nhmode && northHemisMode && newAz < AZ_MAXNHMODE) {
      newAz += 360;
      Serial.print("NH Positive Mode: azimuth adjusted to ");
      Serial.println(newAz);
   } else {
      northHemisMode = false;
      Serial.println("Northern Hemisphere Mode is off");
   }
   
   
   int azDegLeft = abs(newAz - this->azPos);
   int elDegLeft = abs(newEl - this->elPos);
   int azStepMode = AZ_DEFAULTSTEPSIZE; this->azDriver->setStepMode(azStepMode);
   int elStepMode = EL_DEFAULTSTEPSIZE; this->elDriver->setStepMode(elStepMode);
   long azStepsLeft = this->azDegToStep(azDegLeft) / stepModeMultiplierToMicro[azStepMode];
   long elStepsLeft = this->elDegToStep(elDegLeft) / stepModeMultiplierToMicro[elStepMode];
   long azStepsComplete = 0, elStepsComplete = 0;
   long azStepDelay = 0;
   long elStepDelay = 0;
   long azSPS = AZ_STARTINGSPS, elSPS = EL_STARTINGSPS;
   
   // Set direction and print
   Serial.print("az change: ");
   // set azimuth direction
   if (newAz >= this->azPos) {
      this->azDriver->setDirection(StepperDriver::CLOCKWISE);
      Serial.print("+");
   } else {
      this->azDriver->setDirection(StepperDriver::COUNTERCLOCKWISE);
      Serial.print("-");
   }
   Serial.print(azDegLeft);
   Serial.print("; el change: ");
   // set elevation direction
   if (newEl >= this->elPos) {
      this->elDriver->setDirection(StepperDriver::CLOCKWISE);
      Serial.print("+");
   } else {
      this->elDriver->setDirection(StepperDriver::COUNTERCLOCKWISE);
      Serial.print("-");
   }
   Serial.print(elDegLeft);
   Serial.print("; azSteps: ");
   Serial.print(azStepsLeft);
   Serial.print("; elSteps: ");
   Serial.print(elStepsLeft);
   Serial.print("; azSPS: ");
   Serial.print(azSPS);
   Serial.print("; elSPS: ");
   Serial.println(elSPS);

   delay(10);
   this->azDriver->enableDriver();
   this->elDriver->enableDriver();
   delay(10);
   
   
   long azDecelSteps = 0, elDecelSteps = 0; // number of steps to decelerate
   while(elStepsLeft > 0 || azStepsLeft > 0) {
      azStepDelay = floor(MICROSINSECONDS / azSPS);
      elStepDelay = floor(MICROSINSECONDS / elSPS);
      
      if (azStepsLeft == 0) {
         this->azDriver->disableDriver();
      }
      // step once in azimuth if az should step during this millisecond
      // move only if the appropriate delay has passed
      else if (azStepsLeft > 0 && micros() - this->azDriver->getTimeLastStep() >= azStepDelay) {
         // decrement the steps left
         azStepsLeft--;
         azStepsComplete++;
         // step the motor, sets this->timeLastStep
         this->azDriver->stepMotor();
         
         // decelerate
         if (azStepsLeft < azDecelSteps && azSPS > AZ_MINSPS) {
            azSPS -= AZ_SPSPS;
            //Serial.println(azSPS);
         // accelerate
         } else if (azSPS < AZ_MAXSPS && azStepsLeft > azDecelSteps) {
            azSPS += AZ_SPSPS;
            //Serial.println(azSPS);
         }
         // decide when to start decelerating
         if (azDecelSteps == 0 && (azSPS >= AZ_MAXSPS || azStepsLeft == azStepsComplete)) {
            azDecelSteps = azStepsComplete;
            //Serial.print("azDecelSteps=");
            //Serial.println(azDecelSteps);
         }
      }
      
      if (elStepsLeft == 0) {
         this->elDriver->disableDriver();
      }
      // step once in elevation if el should step during this millisecond
      // move only if the appropriate delay has passed
      else if (elStepsLeft > 0 && micros() - this->elDriver->getTimeLastStep() >= elStepDelay) {
         // decrement the steps left
         elStepsLeft--;
         elStepsComplete++;
         // step the motor, sets this->timeLastStep
         this->elDriver->stepMotor();
         
         // decelerate
         if (elStepsLeft < elDecelSteps && elSPS > EL_MINSPS) {
            elSPS -= EL_SPSPS;
            //Serial.println(elSPS);
         // accelerate
         } else if (elSPS < EL_MAXSPS && elStepsLeft > elDecelSteps) {
            elSPS += EL_SPSPS;
            //Serial.println(elSPS);
         }
         // decide when to start decelerating
         if (elDecelSteps == 0 && (elSPS >= EL_MAXSPS || elStepsLeft == elStepsComplete)) {
            elDecelSteps = elStepsComplete;
            //Serial.print("elDecelSteps=");
            //Serial.println(elDecelSteps);
         }
      }
   }
   updatePosition(newAz, newEl);
   delay(300);
   this->azDriver->disableDriver();
   this->elDriver->disableDriver();
   
   return 1;
}

/*

*/
long StandInstance::stepAzimuth(long steps) {
   if (steps >= 0)
      this->azDriver->setDirection(StepperDriver::CLOCKWISE);
   else
      this->azDriver->setDirection(StepperDriver::COUNTERCLOCKWISE);
      
   Serial.print("Step ");
   Serial.print(steps);
   Serial.println(" steps in azimuth");
   steps = abs(steps);
   if (steps != this->azDriver->step(steps, AZ_DEFAULTSTEPSIZE, AZ_STARTINGSPS))
      return 0;
   return 1;
}

long StandInstance::stepElevation(long steps) {
   if (steps >= 0)
      this->elDriver->setDirection(StepperDriver::CLOCKWISE);
   else
      this->elDriver->setDirection(StepperDriver::COUNTERCLOCKWISE);

   Serial.print("Step ");
   Serial.print(steps);
   Serial.println(" steps in elevation");
   steps = abs(steps);
   if (steps != this->elDriver->step(steps, EL_DEFAULTSTEPSIZE, EL_STARTINGSPS))
      return 0;
   return 1;
}

/*
   resetPosition
   Moves Stand to reset position.
 */
void StandInstance::resetPosition()
{
   moveToAccel(AZRESETPOS,ELRESETPOS, false);
}

/*
   getAzimuth
   Returns the current position in azimuth in degrees.
 */
int StandInstance::getAzimuth()
{
   return this->azPos;
}

/*
   getElevation
   Returns the current position in elevation in degrees.
 */
int StandInstance::getElevation()
{
   return this->elPos;
}

/*
   azDegToSteps
   Returns the number of steps from the degrees input.
 */
 long StandInstance::azDegToStep(int degrees)
 {
   long numSteps = degrees * AZ_GEARRATIO * AZ_MICSTEPPERDEG;
   return numSteps;
 }
 
 /*
   elDegToSteps
   Returns the number of steps from the degrees input.
 */
 
 long StandInstance::elDegToStep(int degrees)
 {
   long numSteps = (long) degrees * EL_GEARRATIO * EL_MICSTEPPERDEG;
   return numSteps;
 }
 
 /*
   isValid
   Returns if the position is valid.
 */
 
 bool StandInstance::isValidAzPos(int newAz)
 {
   if(newAz > AZ_MAXPOS || newAz < AZ_MINPOS)
   {
      return false;
   }
   return true;
 }
 
  bool StandInstance::isValidElPos(int newEl)
 {
   if(newEl > EL_MAXPOS || newEl < EL_MINPOS)
   {
      return false;
   }
   return true;
 }